#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/common/common.hh>

namespace gazebo {
class AirDropPlugin : public ModelPlugin {
private:
    physics::ModelPtr model;
    physics::LinkPtr link;
    event::ConnectionPtr updateConnection;
    ignition::math::Pose3d fixedPose;
    bool isFixed = true;
    transport::NodePtr node;
    transport::SubscriberPtr sub;

    std::string link_name_;

public:
    void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
        
        model = _parent;

        if (_sdf->HasElement("linkName"))
            link_name_ = _sdf->GetElement("linkName")->Get<std::string>();
        else
            gzerr << "[airdrop_plugin] Please specify a linkName of the object.\n";
            link = model->GetLink(link_name_);
        
        if (link == NULL)
            gzthrow("[airdrop_plugin] Couldn't find specified link \"" << link_name_ << "\".");
        
        fixedPose = link->WorldPose();

        updateConnection = event::Events::ConnectWorldUpdateBegin(
            std::bind(&AirDropPlugin::OnUpdate, this));
        
        node = transport::NodePtr(new transport::Node());
        node->Init(model->GetWorld()->Name());
        sub = node->Subscribe("~/release", &AirDropPlugin::ReleaseCallback, this);
    }

    void OnUpdate() {
        if (isFixed) {
            link->SetWorldPose(fixedPose);
            link->SetLinearVel(ignition::math::Vector3d(0, 0, 0));
            link->SetAngularVel(ignition::math::Vector3d(0, 0, 0));
        }
    }

    void ReleaseCallback(ConstIntPtr &msg) {
      if (msg->data() == 1) {
          isFixed = false;
          gzmsg << "Box released!\n";
      }
  }
};


GZ_REGISTER_MODEL_PLUGIN(AirDropPlugin)
}
